/*
 * PathFinder.cpp
 *
 *  Created on: 2.2.2013
 *      Author: Atomic
 */

#include "PathFinder.h"
#include "../objects/Entity.h"

int onClosedList = 10;

const int notfinished = 0, notStarted = 0; // pt constant
const int found = 1, nonexistent = 2;
const int walkable = 0, unwalkable = 1; // walkability array constants
const int mapWidth = 257;
const int mapHeight = 257;
const int numberPeople = 1;

PathFinder::PathFinder(Terrain::Terrain *_terrain)
{
	terrain = _terrain;

	int terSize = terrain->get_size();

	//NODE ELEMENT INITIALIZATION
	node = new Node*[terSize + 2];
	for (int i = 0; i < terSize + 2; i++)
	{
		node[i] = new Node[terSize + 2];
	}

	for (int i = 0; i < terSize + 2; i++)
	{
		for (int j = 0; j < terSize + 2; j++)
		{
			node[i][j].object = NULL;
			node[i][j].terrainCost = 0;
			node[i][j].walkability = 0;
		}
	}

	//Array initialization

	openList = new int[terSize * terSize + 2];
	openX = new int[terSize * terSize + 2];
	openY = new int[terSize * terSize + 2];
	Hcost = new int[terSize * terSize + 2];
	Fcost = new int[terSize * terSize + 2];

	whichList = new int*[terSize + 2];
	for (int i = 0; i < terSize + 2; i++)
	{
		whichList[i] = new int[terSize + 2];
	}

	parentX = new int*[terSize + 2];
	for (int i = 0; i < terSize + 2; i++)
	{
		parentX[i] = new int[terSize + 2];
	}

	parentY = new int*[terSize + 2];
	for (int i = 0; i < terSize + 2; i++)
	{
		parentY[i] = new int[terSize + 2];
	}

	Gcost = new int*[terSize + 2];
	for (int i = 0; i < terSize + 2; i++)
	{
		Gcost[i] = new int[terSize + 2];
	}

	//Create the semaphore
	LockReservePos = SDL_CreateSemaphore(1);
	LockFindPath = SDL_CreateSemaphore(1);
	LockFreePos = SDL_CreateSemaphore(1);
	LockGetPath = SDL_CreateSemaphore(1);
}

PathFinder::~PathFinder()
{
}


void PathFinder::updateMapData(float waterLevel)
{
	int size = terrain->get_size();

	for (int x = 0; x < size; x++)
		for (int y = 0; y < size; y++)
		{
			//offset by one.
			if (terrain->get_value(x, y) > waterLevel)
			{
				node[x + 1][y + 1].walkability = 0;
				node[x + 1][y + 1].terrainCost = 1;
			}
			else
			{

				node[x + 1][y + 1].walkability = 2;
				node[x + 1][y + 1].terrainCost = 5;
			}
		}
}



std::list<glm::vec2> PathFinder::getPath(glm::vec2 targetVector,
		glm::vec2 beginVector)
{


	//###########################################################
	// CRITICAL SECTION !!!
	SDL_SemWait(LockGetPath);

	float step = terrain->get_step();

	std::list<glm::vec2> path;
	path = std::list<glm::vec2>();

	int tx = (targetVector.x / step);
	int ty = (targetVector.y / step);
	int bx = (beginVector.x / step);
	int by = (beginVector.y / step);



	if (node[tx][ty].walkability == 1)
	{
		glm::vec2 tempVector = targetVector;
		targetVector = getNearestFreePoint(tempVector);
		tx = (targetVector.x / step);
		ty = (targetVector.y / step);


	}

	InitializePathfinder();
	if (FindPath(1, bx, by, tx, ty) == 1)
	{

		for (int i = 0; i < pathLength[1] * 2; i += 2)
		{
			int px, py;
			px = pathBank[1][i];
			py = pathBank[1][i + 1];

			path.push_front(glm::vec2(px, py));

		}

	}
	EndPathfinder();

	SDL_SemPost(LockGetPath);
	//###########################################################

	return path;
}

void PathFinder::InitializePathfinder(void)
{
	for (int x = 0; x < numberPeople + 1; x++)
		pathBank[x] = (int*) malloc(4);
}

void PathFinder::EndPathfinder(void)
{
	for (int x = 0; x < numberPeople + 1; x++)
	{
		free(pathBank[x]);
	}
}

int PathFinder::FindPath(int pathfinderID, int startingX, int startingY,
		int targetX, int targetY)
{

	//###########################################################
	// CRITICAL SECTION !!!
	SDL_SemWait(LockFindPath);

	int onOpenList = 0, parentXval = 0, parentYval = 0, a = 0, b = 0, m = 0,
			u = 0, v = 0, temp = 0, corner = 0, numberOfOpenListItems = 0,
			addedGCost = 0, tempGcost = 0, path = 0, tempx, pathX, pathY,
			cellPosition, newOpenListItemID = 0;

	int startX = startingX;
	int startY = startingY;



	//	If starting location and target are in the same location...
	if (startX == targetX && startY == targetY && pathLocation[pathfinderID] > 0)
		return found;
	if (startX == targetX && startY == targetY && pathLocation[pathfinderID] == 0)
		return nonexistent;

	//	If target square is unwalkable, return that it's a nonexistent path.
	//  if (node[targetX][targetY].walkability == unwalkable || node[targetX][targetY].object != NULL)
	//	goto noPath;

	//clear buffers
	if (onClosedList > 1000000)
	{
		for (int x = 0; x < mapWidth; x++)
		{
			for (int y = 0; y < mapHeight; y++)
				whichList[x][y] = 0;
		}
		onClosedList = 10;
	}
	onClosedList = onClosedList + 2; //changing the values of onOpenList and onClosed list is faster than redimming whichList() array
	onOpenList = onClosedList - 1;
	pathLength[pathfinderID] = notStarted; //i.e, = 0
	pathLocation[pathfinderID] = notStarted; //i.e, = 0
	Gcost[startX][startY] = 0; //reset starting square's G value to 0

	//Add startiong point to open list
	numberOfOpenListItems = 1;
	openList[1] = 1; //assign it as the top (and currently only) item in the open list, which is maintained as a binary heap (explained below)
	openX[1] = startX;
	openY[1] = startY;

	//Begin A*
	do
	{

		if (numberOfOpenListItems != 0)
		{

			//Take the first item(lowest F value)
			parentXval = openX[openList[1]];
			parentYval = openY[openList[1]]; //record cell coords of the item
			whichList[parentXval][parentYval] = onClosedList; //add the item to the closed list
			numberOfOpenListItems = numberOfOpenListItems - 1; //reduce number of open list items by 1

			//	Delete the top item in binary heap and reorder the heap, with the lowest F cost item rising to the top.
			openList[1] = openList[numberOfOpenListItems + 1];
			v = 1;

			//	reorder the heap, to put new item on open list to right position
			do
			{
				u = v;
				if (2 * u + 1 <= numberOfOpenListItems) //if both children exist
				{
					//Check if the F cost of the parent is greater than each child.
					//Select the lowest of the two children.
					if (Fcost[openList[u]] >= Fcost[openList[2 * u]])
						v = 2 * u;
					if (Fcost[openList[v]] >= Fcost[openList[2 * u + 1]])
						v = 2 * u + 1;
				}
				else
				{
					if (2 * u <= numberOfOpenListItems) //if only child #1 exists
					{
						//Check if the F cost of the parent is greater first child
						if (Fcost[openList[u]] >= Fcost[openList[2 * u]])
							v = 2 * u;
					}
				}

				if (u != v) //if parent's F is > one of its children, swap them
				{
					temp = openList[u];
					openList[u] = openList[v];
					openList[v] = temp;
				}
				else
					break;

			} while (1);


			for (b = parentYval - 1; b <= parentYval + 1; b++)
			{
				for (a = parentXval - 1; a <= parentXval + 1; a++)
				{

					//	bound check
					if (a != -1 && b != -1 && a != mapWidth && b != mapHeight)
					{

						//	not on closed
						if (whichList[a][b] != onClosedList)
						{

							//	walkable
							if (node[a][b].walkability != unwalkable)
							{

								//	no corners
								corner = walkable;
								if (a == parentXval - 1)
								{
									if (b == parentYval - 1)
									{
										if (node[parentXval - 1][parentYval].walkability
												== unwalkable
												|| node[parentXval][parentYval - 1].walkability
														== unwalkable)
											corner = unwalkable;
									}
									else if (b == parentYval + 1)
									{
										if (node[parentXval][parentYval + 1].walkability
												== unwalkable
												|| node[parentXval - 1][parentYval].walkability
														== unwalkable)
											corner = unwalkable;
									}
								}
								else if (a == parentXval + 1)
								{
									if (b == parentYval - 1)
									{
										if (node[parentXval][parentYval - 1].walkability
												== unwalkable
												|| node[parentXval + 1][parentYval].walkability
														== unwalkable)
											corner = unwalkable;
									}
									else if (b == parentYval + 1)
									{
										if (node[parentXval + 1][parentYval].walkability
												== unwalkable
												|| node[parentXval][parentYval + 1].walkability
														== unwalkable)
											corner = unwalkable;
									}
								}
								if (corner == walkable)
								{

									//	If not already on the open list, add it to the open list.
									if (whichList[a][b] != onOpenList)
									{

										//Create a new open list item in the binary heap.
										newOpenListItemID = newOpenListItemID + 1; //each new item has a unique ID #
										m = numberOfOpenListItems + 1;
										openList[m] = newOpenListItemID; //place the new open list item at the bottom of the heap
										openX[newOpenListItemID] = a;
										openY[newOpenListItemID] = b; //record the x and y coordinates of the new item

										//Figure out its G cost
										if (abs(a - parentXval) == 1 && abs(b - parentYval) == 1)
											addedGCost = 14 * node[a][b].terrainCost; //diagonal move
										else
											addedGCost = 10 * node[a][b].terrainCost; //straight move
										Gcost[a][b] = Gcost[parentXval][parentYval] + addedGCost;

										//Figure out its H and F costs and parent
										Hcost[openList[m]] = 10
												* (abs(a - targetX) + abs(b - targetY));
										Fcost[openList[m]] = Gcost[a][b] + Hcost[openList[m]];
										parentX[a][b] = parentXval;
										parentY[a][b] = parentYval;

										//Keep the heap allright :)
										//Reorder it
										while (m != 1)
										{
											//Check if child's F cost is < parent's F cost. If so, swap them.
											if (Fcost[openList[m]] <= Fcost[openList[m / 2]])
											{
												temp = openList[m / 2];
												openList[m / 2] = openList[m];
												openList[m] = temp;
												m = m / 2;
											}
											else
												break;
										}
										numberOfOpenListItems = numberOfOpenListItems + 1; //add one to the number of items in the heap

										//Change whichList to show that the new item is on the open list.
										whichList[a][b] = onOpenList;
									}


									else //If whichList(a,b) = onOpenList
									{

										//if new path si better to this point, renew values
										if (abs(a - parentXval) == 1 && abs(b - parentYval) == 1)
											addedGCost = 14 * node[a][b].terrainCost; //diagonal
										else
											addedGCost = 10 * node[a][b].terrainCost; //straight
										tempGcost = Gcost[parentXval][parentYval] + addedGCost;

										if (tempGcost < Gcost[a][b]) //if G cost is less,
										{
											parentX[a][b] = parentXval; //change the square's parent
											parentY[a][b] = parentYval;
											Gcost[a][b] = tempGcost; //change the G cost


											for (int x = 1; x <= numberOfOpenListItems; x++)
											{
												if (openX[openList[x]] == a && openY[openList[x]] == b)
												{
													Fcost[openList[x]] = Gcost[a][b] + Hcost[openList[x]];
													m = x;
													while (m != 1) //While item hasn't bubbled to the top (m=1)
													{
														//Check if child is < parent. If so, swap them.
														if (Fcost[openList[m]] < Fcost[openList[m / 2]])
														{
															temp = openList[m / 2];
															openList[m / 2] = openList[m];
															openList[m] = temp;
															m = m / 2;
														}
														else
															break;
													}
													break;
												}
											}
										}

									}
								}
							}
						}
					}
				}
			}

		}


		else
		{
			path = nonexistent;
			break;
		}


		if (whichList[targetX][targetY] == onOpenList)
		{
			path = found;
			break;
		}

	} while (1);


	if (path == found)
	{

		pathX = targetX;
		pathY = targetY;
		do
		{
			//Look up the parent of the current cell.
			tempx = parentX[pathX][pathY];
			pathY = parentY[pathX][pathY];
			pathX = tempx;
			//Figure out the path length
			pathLength[pathfinderID] = pathLength[pathfinderID] + 1;
		} while (pathX != startX || pathY != startY);


		pathBank[pathfinderID] = (int*) realloc(pathBank[pathfinderID],
				pathLength[pathfinderID] * 8);

		pathX = targetX;
		pathY = targetY;
		cellPosition = pathLength[pathfinderID] * 2; //start at the end
		do
		{
			cellPosition = cellPosition - 2;
			pathBank[pathfinderID][cellPosition] = pathX;
			pathBank[pathfinderID][cellPosition + 1] = pathY;

			tempx = parentX[pathX][pathY];
			pathY = parentY[pathX][pathY];
			pathX = tempx;

		} while (pathX != startX || pathY != startY);

		ReadPath(pathfinderID, startingX, startingY, 1);
	}

	SDL_SemPost(LockFindPath);
	//###########################################################

	return path;

	noPath: xPath[pathfinderID] = startingX;
	yPath[pathfinderID] = startingY;
	return nonexistent;
}

void PathFinder::ReadPath(int pathfinderID, int currentX, int currentY,
		int pixelsPerFrame)
{

	int ID = pathfinderID;


	if (pathStatus[ID] == found)
	{

		if (pathLocation[ID] < pathLength[ID])
		{

			if (pathLocation[ID] == 0
					|| (abs(currentX - xPath[ID]) < pixelsPerFrame
							&& abs(currentY - yPath[ID]) < pixelsPerFrame))
				pathLocation[ID] = pathLocation[ID] + 1;
		}

		//Read the path data.
		xPath[ID] = ReadPathX(ID, pathLocation[ID]) + 1; //plus one because of the map bounds
		yPath[ID] = ReadPathY(ID, pathLocation[ID]) + 1; //plus one because of the map bounds


		if (pathLocation[ID] == pathLength[ID])
		{
			if (abs(currentX - xPath[ID]) < pixelsPerFrame
					&& abs(currentY - yPath[ID]) < pixelsPerFrame)
				pathStatus[ID] = notStarted;
		}
	}

	else
	{
		xPath[ID] = currentX;
		yPath[ID] = currentY;
	}
}

int PathFinder::ReadPathX(int pathfinderID, int pathLocation)
{
	int x;
	if (pathLocation <= pathLength[pathfinderID])
	{
		//Read coordinate from bank
		x = pathBank[pathfinderID][pathLocation * 2 - 2];

	}
	return x;
}

int PathFinder::ReadPathY(int pathfinderID, int pathLocation)

{
	int y;
	if (pathLocation <= pathLength[pathfinderID])
	{
		//Read coordinate from bank
		y = pathBank[pathfinderID][pathLocation * 2 - 1];

	}
	return y;
}

int PathFinder::ReservePosition(int x, int y, void *entityObject)
{
	//###########################################################
	// CRITICAL SECTION !!!
	SDL_SemWait(LockReservePos);

	int out = 0;

	Entity *obj = static_cast<Entity*>(entityObject);

	if (node[x][y].walkability == 0)
	{
		node[x][y].walkability = 1;

		out = 1;
		node[x][y].object = obj;
	}
	else if (node[x][y].walkability == 2)
	{
		out = 69; // if water, that means Y U never get here, human! :D
	}
	else
		out = 0;

	SDL_SemPost(LockReservePos);
	//###########################################################

	return out;
}

int PathFinder::ReservePosition(int x, int y)
{
	//###########################################################
	// CRITICAL SECTION !!!
	SDL_SemWait(LockReservePos);

	int out = 0;

	if (node[x][y].walkability == 0)
	{
		node[x][y].walkability = 1;
		out = 1;
	}
	else if (node[x][y].walkability == 2)
	{
		out = 69; // if water, that means Y U never get here, human! :D
	}
	else
		out = 0;

	SDL_SemPost(LockReservePos);
	//###########################################################

	return out;
}

void PathFinder::FreePosition(int x, int y)
{
	//###########################################################
	// CRITICAL SECTION !!!
	SDL_SemWait(LockFreePos);

	node[x][y].walkability = 0;
	node[x][y].object = NULL;
	SDL_SemPost(LockFreePos);
	//###########################################################
}

bool PathFinder::CheckBuildingPosition(DataTypes::Cube *cubePlace,
		glm::vec2 pos)
{
	int x, y, xSize, ySize;
	if (cubePlace != NULL)
	{
		x = (int) ((pos.x - abs(cubePlace->getVertexes()[BOTTOM_LEFT_D].x))
				/ terrain->get_step());
		y = (int) ((pos.y - abs(cubePlace->getVertexes()[BOTTOM_LEFT_D].z))
				/ terrain->get_step());
		xSize = (int) (((cubePlace->getVertexes()[BOTTOM_RIGHT_D].x + pos.x)
				/ terrain->get_step()) + 1);
		ySize = (int) (((pos.y - cubePlace->getVertexes()[TOP_LEFT_D].z)
				/ terrain->get_step()) + 1);

		// check corner of map
		if (xSize > terrain->get_size())
			xSize = (terrain->get_size());
		if (ySize > terrain->get_size())
			ySize = terrain->get_size();

		for (int xT = x; xT < xSize; xT++)
		{
			for (int yT = y; yT < ySize; yT++)
			{
				if (node[xT][yT].walkability != 0)
					return false;
			}
		}

		return true;
	}
	return false;
}
bool PathFinder::ReserveBuildingPosition(DataTypes::Cube *cubePlace,
		glm::vec2 pos, void* obj)
{
	int x, y, xSize, ySize;

	if (cubePlace != NULL)
	{
		x = (int) ((pos.x - abs(cubePlace->getVertexes()[BOTTOM_LEFT_D].x))
				/ terrain->get_step());
		y = (int) ((pos.y - abs(cubePlace->getVertexes()[BOTTOM_LEFT_D].z))
				/ terrain->get_step());
		xSize = (int) (((cubePlace->getVertexes()[BOTTOM_RIGHT_D].x + pos.x)
				/ terrain->get_step()) + 1);
		ySize = (int) (((pos.y - cubePlace->getVertexes()[TOP_LEFT_D].z)
				/ terrain->get_step()) + 1);

		// check corner of map
		if (xSize > terrain->get_size())
			xSize = (terrain->get_size());
		if (ySize > terrain->get_size())
			ySize = terrain->get_size();

		for (int xT = x; xT < xSize; xT++)
		{
			for (int yT = y; yT < ySize; yT++)
			{
				ReservePosition(xT, yT, obj);
			}
		}
		return true;
	}
	return false;
}

void* PathFinder::getNearestEntityOfType(glm::vec2 pos, int r, int objtype)
{
	return getNearestEntityOfType(pos, r, objtype, -1, false);
}

void* PathFinder::getNearestEntityOfType(glm::vec2 pos, int r, int type,
		int status, bool chckParetType)
{
	int posx = (int) pos.x;
	int posy = (int) pos.y;

	// no node reserved when entity sleeps
	if (status == STATUS_SLEEPING)
		return NULL;

	for (int rad = 1; rad < r; rad++)
	{

		//For X changing on both sides
		for (int x = posx - rad; x <= posx + rad; x++)
		{
			if (x > terrain->get_size() || x < 0 || (posy - rad) < 0
					|| (posy + rad) > terrain->get_size())
				break;

			if (node[x][posy - rad].object != NULL)
			{
				Entity *obj = static_cast<Entity*>(node[x][posy - rad].object);

				if ((obj->getType() == type) && (status == -1) && (!chckParetType))
					return node[x][posy - rad].object;
				if ((obj->getType() == type) && (obj->getStatus() == status)
						&& (!chckParetType))
					return node[x][posy - rad].object;
				if ((obj->getParentType() == type) && (obj->getStatus() == status)
						&& (chckParetType))
					return node[x][posy - rad].object;

			}

			if (node[x][posy + rad].object != NULL)
			{
				Entity *obj = static_cast<Entity*>(node[x][posy + rad].object);

				if ((obj->getType() == type) && (status == -1) && (!chckParetType))
					return node[x][posy + rad].object;
				if ((obj->getType() == type) && (obj->getStatus() == status)
						&& (!chckParetType))
					return node[x][posy + rad].object;
				if ((obj->getParentType() == type) && (obj->getStatus() == status)
						&& (chckParetType))
					return node[x][posy + rad].object;
			}
		}

		//For Y changing value on both sides

		for (int y = posy - rad; y <= posy + rad; y++)
		{
			if (y > terrain->get_size() || y < 0 || (posx - rad) < 0
					|| (posx + rad) > terrain->get_size())
				break;
			if (node[posx + rad][y].object != NULL)
			{
				Entity *obj = static_cast<Entity*>(node[posx + rad][y].object);
				if ((obj->getType() == type) && (status == -1) && (!chckParetType))
					return node[posx + rad][y].object;
				if ((obj->getType() == type) && (obj->getStatus() == status)
						&& (!chckParetType))
					return node[posx + rad][y].object;
				if ((obj->getParentType() == type) && (obj->getStatus() == status)
						&& (chckParetType))
					return node[posx + rad][y].object;

			}

			if (node[posx - rad][y].object != NULL)
			{
				Entity *obj = static_cast<Entity*>(node[posx - rad][y].object);
				if ((obj->getType() == type) && (status == -1) && (!chckParetType))
					return node[posx - rad][y].object;
				if ((obj->getType() == type) && (obj->getStatus() == status)
						&& (!chckParetType))
					return node[posx - rad][y].object;
				if ((obj->getParentType() == type) && (obj->getStatus() == status)
						&& (chckParetType))
					return node[posx - rad][y].object;
			}
		}
	}

	return NULL;
}

bool PathFinder::uniqueInVector(std::vector<void *> input, void* data)
{
	for (unsigned int i = 0; i < input.size(); i++)
	{
		if (input[i] == data)
			return false;
	}

	return true;
}

std::vector<void*> PathFinder::getEntityOfTypeInArea(glm::vec2 pos, int r,
		int type, int status)
{
	int posx = (int) pos.x;
	int posy = (int) pos.y;

	std::vector<void*> bufVec;
	bufVec = std::vector<void *>();

	// no node reserved when entity sleeps
	if (status == STATUS_SLEEPING)
		return bufVec;

	for (int rad = 1; rad < r; rad++)
	{

		//For X changing on both sides
		for (int x = posx - rad; x <= posx + rad; x++)
		{
			if (x > terrain->get_size() || x < 0 || (posy - rad) < 0
					|| (posy + rad) > terrain->get_size())
				break;

			if (node[x][posy - rad].object != NULL)
			{
				Entity *obj = static_cast<Entity*>(node[x][posy - rad].object);

				if (uniqueInVector(bufVec, node[x][posy - rad].object))
				{
					if ((obj->getParentType() == type) && (obj->getStatus() == status))
						bufVec.push_back(node[x][posy - rad].object);
				}
			}

			if (node[x][posy + rad].object != NULL)
			{
				Entity *obj = static_cast<Entity*>(node[x][posy + rad].object);

				if (uniqueInVector(bufVec, node[x][posy + rad].object))
				{
					if ((obj->getParentType() == type) && (obj->getStatus() == status))
						bufVec.push_back(node[x][posy + rad].object);
				}
			}
		}

		//For Y changing value on both sides

		for (int y = posy - rad; y <= posy + rad; y++)
		{
			if (y > terrain->get_size() || y < 0 || (posx - rad) < 0
					|| (posx + rad) > terrain->get_size())
				break;

			if (node[posx + rad][y].object != NULL)
			{
				Entity *obj = static_cast<Entity*>(node[posx + rad][y].object);

				if (uniqueInVector(bufVec, node[posx + rad][y].object))
				{
					if ((obj->getParentType() == type) && (obj->getStatus() == status))
						bufVec.push_back(node[posx + rad][y].object);
				}
			}

			if (node[posx - rad][y].object != NULL)
			{
				Entity *obj = static_cast<Entity*>(node[posx - rad][y].object);

				if (uniqueInVector(bufVec, node[posx - rad][y].object))
				{
					if ((obj->getParentType() == type) && (obj->getStatus() == status))
						bufVec.push_back(node[posx - rad][y].object);
				}
			}
		}
	}

	return bufVec;
}

glm::vec2 PathFinder::getNearestFreePoint(glm::vec2 pos)
{
	int posx = (int) pos.x;
	int posy = (int) pos.y;
	glm::vec2 ret;
	ret.x = 5;
	ret.y = 5;

	for (int rad = 1; rad < 10; rad++)
	{

		//For X changing on both sides
		for (int x = posx - rad; x <= posx + rad; x++)
		{
			if (x > terrain->get_size() || x < 0 || (posy - rad) < 0
					|| (posy + rad) > terrain->get_size())
				break;

			if (node[x][posy - rad].walkability == 0)
			{
				ret.x = x;
				ret.y = posy - rad;
				return ret;
			}

			if (node[x][posy + rad].walkability == 0)
			{
				ret.x = x;
				ret.y = posy + rad;
				return ret;
			}
		}

		//For Y changing value on both sides

		for (int y = posy - rad; y <= posy + rad; y++)
		{
			if (y > terrain->get_size() || y < 0 || (posx - rad) < 0
					|| (posx + rad) > terrain->get_size())
				break;

			if (node[posx - rad][y].walkability == 0)
			{
				ret.x = posx - rad;
				ret.y = y;
				return ret;
			}
			if (node[posx + rad][y].walkability == 0)
			{
				ret.x = posx + rad;
				ret.y = y;
				return ret;
			}
		}
	}
	return ret;
}

